'''
用于处理接收和发送数据的类
'''

from log_info import LogError , LogInfo , LogWarning
import struct
from data_typedef import Imu_Data,Debug_Data,Robot_Info_Data,Event_Data,Robot_Cmd_Data,Pid_Debug_Data,   \
                         All_Robot_HP_Data,Game_Status_Data,Robot_Motion_Data,Ground_Robot_Position_Data,\
                         Rfid_Status_Data,Rfid_Status_Data,Joint_State_Data,Buff_Data
                         
from data_typedef import SEND_ID_ROBOT_CMD,SEND_ID_VIRTUAL_RC
import lib.CRC8_CRC16 as crc

TIME_STAMP_OFFEST = 4

class Data_Process():
    def __init__(self) -> None:
        # receive data
        self.imu_data = Imu_Data()
        self.debug_data = Debug_Data()
        self.robot_info_data = Robot_Info_Data()
        self.event_data = Event_Data()
        self.pid_debug_data = Pid_Debug_Data()
        self.all_robot_hp = All_Robot_HP_Data()
        self.game_status_data = Game_Status_Data()
        self.robot_motion_data = Robot_Motion_Data()
        self.ground_robot_positin = Ground_Robot_Position_Data()
        self.rfid_status = Rfid_Status_Data()
        self.robot_status = Rfid_Status_Data()
        self.joint_state = Joint_State_Data()
        self.buff= Buff_Data()

        # send data
        self.robot_cmd_data = Robot_Cmd_Data()
        self.timestamp = 0
        
        self.sending_data = False
        return 

    def start_send(self):
        self.sending_data = True
        return
    
    def stop_send(self):
        self.sending_data = False
        return

############################################################
#  数据处理基本功能
#  receive 对接收到的数据进行处理并存储
#  send 发送数据
#  clear 清空数据
############################################################

    def receive(self,received_data:bytes):
        # 解码帧头信息
        data_len = int(received_data[1])
        data_id = received_data[2]
        # print(f'接收到数据, 数据ID:{data_id}，数据长度:{data_len}')
        
        if data_id == 0: # 
            pass
        elif data_id == 1: # Debug数据
            data_dict = {}
            data_num = data_len//15 # 数据个数
            offset = 8
            for i in range(data_num):
                data_name = received_data[offset:offset+10].rstrip(b'\0').decode('utf-8')
                data_type = received_data[offset+10]
                raw_data  = received_data[offset+11:offset+15]
                if data_type == 0:
                    data = struct.unpack('<I', raw_data)[0]
                elif data_type == 1:
                    data = struct.unpack('<f', raw_data)[0]
                else:
                    data = 0
                    LogError("未知数据类型")
                # print(data_name,data_type,data)
                # print(data_name == '')
                offset += 15
                if data_name != '':
                    data_dict[data_name] = data

            debug = {
                "time_stamp":(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),
                "datas":data_dict
            }
            self.debug_data.update(debug)
            # print(self.debug_data.latest)
            
        elif data_id == 2: # Imu数据
            # imu数据的偏移量
            YAW_OFFEST = 8
            PITCH_OFFEST = 12
            ROLL_OFFEST = 16

            YAW_VEL_OFFEST = 20
            PITCH_VEL_OFFEST = 24
            ROLL_VEL_OFFEST = 28

            imu = {
                "time_stamp":(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),
                
                "yaw"  :struct.unpack('<f', received_data[YAW_OFFEST   : YAW_OFFEST + 4])[0],
                "pitch":struct.unpack('<f', received_data[PITCH_OFFEST : PITCH_OFFEST + 4])[0],
                "roll" :struct.unpack('<f', received_data[ROLL_OFFEST  : ROLL_OFFEST + 4])[0],
                
                "yaw_vel"  :struct.unpack('<f', received_data[YAW_VEL_OFFEST   : YAW_VEL_OFFEST + 4])[0],
                "pitch_vel":struct.unpack('<f', received_data[PITCH_VEL_OFFEST : PITCH_VEL_OFFEST + 4])[0],
                "roll_vel" :struct.unpack('<f', received_data[ROLL_VEL_OFFEST  : ROLL_VEL_OFFEST + 4])[0],
            }
            
            self.imu_data.update(imu)
            # print(self.imu_data.latest)
            
        elif data_id == 3: # 机器人信息数据
            TYPES_OFFSET = 8
            STATE_OFFSET = 10
            # ID_OFFSET = 11
            # COLOR_OFFSET= 12
            # ATTACKED_OFFSET = 13
            # HP_OFFSET= 14
            # HEAT_OFFSET= 16
            
            chassis_names = ['无底盘','麦轮底盘','全向轮底盘','舵轮底盘','平衡底盘']
            gimbal_names = ['无云台','yaw-pitch电机直连云台']
            shoot_names = ['无发射机构','摩擦轮+拨弹盘','气动+拨弹盘']
            arm_names = ['无机械臂','企鹅mini机械臂']
            custom_controller_names = ['无自定义控制器','企鹅mini自定义控制器']
            
            time_stamp = struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]
            
            types_raw = struct.unpack('<H', received_data[TYPES_OFFSET : TYPES_OFFSET+2])[0]
            types = {
                'chassis':           (types_raw>>0 ) & 0b111,
                'gimbal':            (types_raw>>3 ) & 0b111,
                'shoot':             (types_raw>>6 ) & 0b111,
                'arm':               (types_raw>>9 ) & 0b111,
                'custom_controller': (types_raw>>12) & 0b111,
            }
            
            state_raw = int(received_data[STATE_OFFSET])
            state = {
                'chassis':           bool((state_raw>>0 ) & 0b1),
                'gimbal':            bool((state_raw>>1 ) & 0b1),
                'shoot':             bool((state_raw>>2 ) & 0b1),
                'arm':               bool((state_raw>>3 ) & 0b1),
                'custom_controller': bool((state_raw>>4 ) & 0b1),
            }
            
            # referee = {
            #     'id':       int(received_data[ID_OFFSET]),
            #     'color':    int(received_data[COLOR_OFFSET]),
            #     'attacked': bool(received_data[ATTACKED_OFFSET]),
            #     'hp':       struct.unpack('<H', received_data[HP_OFFSET:HP_OFFSET+2])[0],
            #     'heat':     struct.unpack('<H', received_data[HEAT_OFFSET:HEAT_OFFSET+2])[0],
            # }
            
            robot_info = {
                'time_stamp': time_stamp,
                'types': types,
                'state': state,
                # 'referee': referee,
            }
            self.robot_info_data.update(robot_info)
            # print(self.robot_info_data.latest)

        elif data_id == 4: # 比赛事件数据
            EVENT_DATA_RAW_OFFSET = 8
            
            event_data_raw = struct.unpack('<H', received_data[ EVENT_DATA_RAW_OFFSET: EVENT_DATA_RAW_OFFSET+2])[0]
            event = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),

                'non_overlapping_supply_zone':    bool((event_data_raw>>0 ) & 0b1),
                'overlapping_supply_zone':       bool((event_data_raw>>1 ) & 0b1),
                'supply_zone':                  bool((event_data_raw>>2 ) & 0b1),

                'small_energy':                 bool((event_data_raw>>3 ) & 0b1),
                'big_energy':                   bool((event_data_raw>>4 ) & 0b1),

                'central_highland':                  ( event_data_raw>>5 ) & 0b11,
                'trapezoidal_highland':              ( event_data_raw>>8 ) & 0b11,

                'center_gain_zone':                  ((event_data_raw>>10) & 0b11),
            } 
            self.event_data.update(event)

        elif data_id == 5: # pid调参数据
            FDB_OFFSET = 8
            REF_OFFSET = 12
            PID_OUT_OFFSET = 16
            pid_debug = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),
                
                'fdb':struct.unpack('<f', received_data[FDB_OFFSET:FDB_OFFSET + 4])[0],
                'ref':struct.unpack('<f', received_data[REF_OFFSET:REF_OFFSET + 4])[0],
                'pid_out':struct.unpack('<f', received_data[PID_OUT_OFFSET:PID_OUT_OFFSET + 4])[0],
            }
            self.pid_debug_data.update(pid_debug)
            
        elif data_id == 6: # 全场机器人HP数据
            RED_1_ROBOT_HP_OFFSET = 8
            RED_2_ROBOT_HP_OFFSET = 10
            RED_3_ROBOT_HP_OFFSET = 12
            RED_4_ROBOT_HP_OFFSET = 14
            RED_7_ROBOT_HP_OFFSET = 16
            RED_OUTPOST_HP_OFFSET = 18
            RED_BASE_HP = 20

            BLUE_1_ROBOT_HP_OFFSET = 22
            BLUE_2_ROBOT_HP_OFFSET = 24
            BLUE_3_ROBOT_HP_OFFSET = 26
            BLUE_4_ROBOT_HP_OFFSET = 28
            BLUE_7_ROBOT_HP_OFFSET = 30
            BLUE_OUTPOST_HP_OFFSET = 32
            BLUE_BASE_HP = 34
            all_robot_hp = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),
                
                'red_1_robot_hp' :struct.unpack('<H', received_data[RED_1_ROBOT_HP_OFFSET:RED_1_ROBOT_HP_OFFSET+2])[0],
                'red_2_robot_hp' :struct.unpack('<H', received_data[RED_2_ROBOT_HP_OFFSET:RED_2_ROBOT_HP_OFFSET+2])[0],
                'red_3_robot_hp' :struct.unpack('<H', received_data[RED_3_ROBOT_HP_OFFSET:RED_3_ROBOT_HP_OFFSET+2])[0],
                'red_4_robot_hp' :struct.unpack('<H', received_data[RED_4_ROBOT_HP_OFFSET:RED_4_ROBOT_HP_OFFSET+2])[0],
                'red_7_robot_hp' :struct.unpack('<H', received_data[RED_7_ROBOT_HP_OFFSET:RED_7_ROBOT_HP_OFFSET+2])[0],
                'red_outpost_hp' :struct.unpack('<H', received_data[RED_OUTPOST_HP_OFFSET:RED_OUTPOST_HP_OFFSET+2])[0],
                'red_base_hp' :struct.unpack('<H', received_data[RED_BASE_HP:RED_BASE_HP+2])[0],

                'blue_1_robot_hp' :struct.unpack('<H', received_data[BLUE_1_ROBOT_HP_OFFSET:BLUE_1_ROBOT_HP_OFFSET+2])[0],
                'blue_2_robot_hp' :struct.unpack('<H', received_data[BLUE_2_ROBOT_HP_OFFSET:BLUE_2_ROBOT_HP_OFFSET+2])[0],
                'blue_3_robot_hp' :struct.unpack('<H', received_data[BLUE_3_ROBOT_HP_OFFSET:BLUE_3_ROBOT_HP_OFFSET+2])[0],
                'blue_4_robot_hp' :struct.unpack('<H', received_data[BLUE_4_ROBOT_HP_OFFSET:BLUE_4_ROBOT_HP_OFFSET+2])[0],
                'blue_7_robot_hp' :struct.unpack('<H', received_data[BLUE_7_ROBOT_HP_OFFSET:BLUE_7_ROBOT_HP_OFFSET+2])[0],
                'blue_outpost_hp' :struct.unpack('<H', received_data[BLUE_OUTPOST_HP_OFFSET:BLUE_OUTPOST_HP_OFFSET+2])[0],
                'blue_base_hp' :struct.unpack('<H', received_data[BLUE_BASE_HP:BLUE_BASE_HP+2])[0],
            }
            self.all_robot_hp.update(all_robot_hp)

        elif data_id == 7: # 比赛信息数据
            GAME_PROGRESS_OFFSET = 8
            STAGE_REMAIN_TIME_OFFSET = 9

            game_status = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),
                
                'game_progress' : int(received_data[GAME_PROGRESS_OFFSET]),
                'stage_remain_time' : struct.unpack('<H', received_data[STAGE_REMAIN_TIME_OFFSET:STAGE_REMAIN_TIME_OFFSET+2])[0],
            }
            self.game_status_data.update(game_status)
            
        elif data_id == 8: # 机器人运动数据
            VX_OFFSET = 8
            VY_OFFSET = 12
            WZ_OFFSET = 16

            robot_motion = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),
                
                'speed_vector': {
                    'vx':struct.unpack('<f', received_data[VX_OFFSET:VX_OFFSET + 4])[0],
                    'vy':struct.unpack('<f', received_data[VY_OFFSET:VY_OFFSET + 4])[0],
                    'wz':struct.unpack('<f', received_data[WZ_OFFSET:WZ_OFFSET + 4])[0],
                }
            }
            self.robot_motion_data.update(robot_motion)
            
        elif data_id == 9: # 地面机器人位置数据
            HERO_X_OFFSET = 8
            HERO_Y_OFFSET = 12
            
            ENGINEER_X_OFFSET = 16
            ENGINEER_Y_OFFSET = 20
            
            STANDARD_3_X_OFFSET = 24
            STANDARD_3_Y_OFFSET = 28
            
            STANDARD_4_X_OFFSET = 32
            STANDARD_4_Y_OFFSET = 36
            
            STANDARD_5_X_OFFSET = 40
            STANDARD_5_Y_OFFSET = 44

            ground_robot_positin = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),
                
                'hero_x':struct.unpack('<f', received_data[HERO_X_OFFSET:HERO_X_OFFSET + 4])[0],
                'hero_y':struct.unpack('<f', received_data[HERO_Y_OFFSET:HERO_Y_OFFSET + 4])[0],

                'engineer_x':struct.unpack('<f', received_data[ENGINEER_X_OFFSET:ENGINEER_X_OFFSET + 4])[0],
                'engineer_y':struct.unpack('<f', received_data[ENGINEER_Y_OFFSET:ENGINEER_Y_OFFSET + 4])[0],

                'standard_3_x':struct.unpack('<f', received_data[STANDARD_3_X_OFFSET:STANDARD_3_X_OFFSET + 4])[0],
                'standard_3_y':struct.unpack('<f', received_data[STANDARD_3_Y_OFFSET:STANDARD_3_Y_OFFSET + 4])[0],

                'standard_4_x':struct.unpack('<f', received_data[STANDARD_4_X_OFFSET:STANDARD_4_X_OFFSET + 4])[0],
                'standard_4_y':struct.unpack('<f', received_data[STANDARD_4_Y_OFFSET:STANDARD_4_Y_OFFSET + 4])[0],

                'standard_5_x':struct.unpack('<f', received_data[STANDARD_5_X_OFFSET:STANDARD_5_X_OFFSET + 4])[0],
                'standard_5_y':struct.unpack('<f', received_data[STANDARD_5_Y_OFFSET:STANDARD_5_Y_OFFSET + 4])[0],
            }
            self.ground_robot_positin.update(ground_robot_positin)
        
        elif data_id == 10: # RFID状态数据
            RFID_DATA_RAW_OFFSET = 8

            rfid_data_raw = (struct.unpack('<I', received_data[RFID_DATA_RAW_OFFSET : RFID_DATA_RAW_OFFSET+4])[0])

            rfid_status = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),

                'base_gain_point' : bool((rfid_data_raw>>0 ) & 0b1),
                'central_highland_gain_point' : bool((rfid_data_raw>>1 ) & 0b1),
                'enemy_central_highland_gain_point' : bool((rfid_data_raw>>2 ) & 0b1),
                'friendly_trapezoidal_highland_gain_point' : bool((rfid_data_raw>>3 ) & 0b1),
                'enemy_trapezoidal_highland_gain_point' : bool((rfid_data_raw>>4 ) & 0b1),
                'friendly_fly_ramp_front_gain_point' : bool((rfid_data_raw>>5 ) & 0b1),
                'friendly_fly_ramp_back_gain_point' : bool((rfid_data_raw>>6 ) & 0b1),
                'enemy_fly_ramp_front_gain_point' : bool((rfid_data_raw>>7 ) & 0b1),
                'enemy_fly_ramp_back_gain_point' : bool((rfid_data_raw>>8 ) & 0b1),
                'friendly_central_highland_lower_gain_point' : bool((rfid_data_raw>>9 ) & 0b1),
                'friendly_central_highland_upper_gain_point' : bool((rfid_data_raw>>10) & 0b1),
                'enemy_central_highland_lower_gain_point' : bool((rfid_data_raw>>11) & 0b1),
                'enemy_central_highland_upper_gain_point' : bool((rfid_data_raw>>12) & 0b1),
                'friendly_highway_lower_gain_point' : bool((rfid_data_raw>>13) & 0b1),
                'friendly_highway_upper_gain_point' : bool((rfid_data_raw>>14) & 0b1),
                'enemy_highway_lower_gain_point' :  bool((rfid_data_raw>>15) & 0b1),
                'enemy_highway_upper_gain_point' : bool((rfid_data_raw>>16) & 0b1),
                'friendly_fortress_gain_point' : bool((rfid_data_raw>>17) & 0b1),
                'friendly_outpost_gain_point' : bool((rfid_data_raw>>18) & 0b1),
                'friendly_supply_zone_non_exchange' : bool((rfid_data_raw>>19) & 0b1),
                'friendly_supply_zone_exchange' : bool((rfid_data_raw>>20) & 0b1),
                'friendly_big_resource_island' : bool((rfid_data_raw>>21) & 0b1),
                'enemy_big_resource_island' : bool((rfid_data_raw>>22) & 0b1),
                'center_gain_point' : bool((rfid_data_raw>>23) & 0b1),
            }
            self.rfid_status.update(rfid_status)
            
        elif data_id == 11: # 机器人状态数据
            ROBOT_ID_OFFSET = 8
            ROBOT_LEVEL_OFFSET = 9
            CURRENT_UP_OFFSET = 10
            MAXIMUM_HP_OFFSET = 12
            SHOOTER_BARREL_COOLING_VALUE_OFFSET = 14
            SHOOTER_BARREL_HEAT_LIMIT = 16

            SHOOTER_17MM_1_BARREL_HEAT = 18

            ROBOT_POS_X_OFFSET = 20
            ROBOT_POS_Y_OFFSET = 24
            ROBOT_POS_ANGLE_OFFSET = 28

            ROBOT_STATUS_DATA_RAW_OFFSET = 32

            PROJECTILE_ALLOWANCE_17MM_1_OFFSET = 33
            REMAINING_GOLD_COIN_OFFSET = 34

            
            
            robot_status_data_raw = int(received_data[ROBOT_STATUS_DATA_RAW_OFFSET])
            
            robot_status = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),
                
                'robot_id':int(received_data[ROBOT_ID_OFFSET]),
                'robot_level':int(received_data[ROBOT_LEVEL_OFFSET]),
                'current_up':struct.unpack('<H', received_data[CURRENT_UP_OFFSET:CURRENT_UP_OFFSET+2])[0],
                'maximum_hp':struct.unpack('<H', received_data[MAXIMUM_HP_OFFSET:MAXIMUM_HP_OFFSET+2])[0],
                'shooter_barrel_cooling_value':struct.unpack('<H', received_data[SHOOTER_BARREL_COOLING_VALUE_OFFSET:SHOOTER_BARREL_COOLING_VALUE_OFFSET+2])[0],
                'shooter_barrel_heat_limit':struct.unpack('<H', received_data[SHOOTER_BARREL_HEAT_LIMIT:SHOOTER_BARREL_HEAT_LIMIT+2])[0],

                'shooter_17mm_1_barrel_heat':struct.unpack('<H', received_data[SHOOTER_17MM_1_BARREL_HEAT:SHOOTER_17MM_1_BARREL_HEAT+2])[0],

                'robot_pos_x':struct.unpack('<f', received_data[ROBOT_POS_X_OFFSET:ROBOT_POS_X_OFFSET + 4])[0],
                'robot_pos_y':struct.unpack('<f', received_data[ROBOT_POS_Y_OFFSET:ROBOT_POS_Y_OFFSET + 4])[0],
                'robot_pos_angle':struct.unpack('<f', received_data[ROBOT_POS_ANGLE_OFFSET:ROBOT_POS_ANGLE_OFFSET + 4])[0],

                'armor_id':(  robot_status_data_raw >> 0 ) & 0b1111,
                'hp_deduction_reason':( robot_status_data_raw >> 4 ) & 0b1111,

                'projectile_allowance_17mm_1':struct.unpack('<H', received_data[PROJECTILE_ALLOWANCE_17MM_1_OFFSET:PROJECTILE_ALLOWANCE_17MM_1_OFFSET+2])[0],
                'remaining_gold_coin':struct.unpack('<H', received_data[REMAINING_GOLD_COIN_OFFSET:REMAINING_GOLD_COIN_OFFSET+2])[0],
            }
            self.robot_status.update(robot_status)
            
        elif data_id == 12: # 云台状态数据
            PITCH1_OFFEST = 8
            YAW1_OFFSET = 12

            joint_state = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),

                'pitch1':struct.unpack('<f', received_data[PITCH1_OFFEST:PITCH1_OFFEST + 4])[0],
                'yaw1':struct.unpack('<f', received_data[YAW1_OFFSET:YAW1_OFFSET + 4])[0],
            }
            self.joint_state.update(joint_state)
            return 

        elif data_id == 13: # buff状态数据
            RECOVERY_BUFF_OFFSET = 8
            COOLING_BUFF_OFFSET = 9
            DEFENCE_BUFF_OFFSET = 10
            VULNERABILITY_BUFF_OFFSET = 11
            ATTACK_BUFF_OFFSET = 12
            REMAINING_ENERGY_OFFSET = 14


            buff = {
                'time_stamp':(struct.unpack('<I', received_data[TIME_STAMP_OFFEST : TIME_STAMP_OFFEST+4])[0]),

                'recovery_buff':int(received_data[RECOVERY_BUFF_OFFSET]),
                'cooling_buff':int(received_data[COOLING_BUFF_OFFSET]),
                'defence_buff':int(received_data[DEFENCE_BUFF_OFFSET]),
                'vulnerability_buff':int(received_data[VULNERABILITY_BUFF_OFFSET]),
                'attack_buff':struct.unpack('<H', received_data[ATTACK_BUFF_OFFSET:ATTACK_BUFF_OFFSET+2])[0],
                'remaining_energy':int(received_data[REMAINING_ENERGY_OFFSET]),
            }
            self.buff.update(buff)
            return

    def send(self,send_id:int,data_dict:dict={})->bytes:
        if not self.sending_data:
            return
        self.timestamp += 1
        if send_id == SEND_ID_ROBOT_CMD:
            send_data = b'\x5a\x2a\x01' # sof + len + id
            send_data = crc.AppendCRC8(send_data)
            
            # 添加时间戳
            data = struct.pack('<I', self.timestamp)
            send_data += data
            # print(len(send_data))
            
            # 添加speed_vector数据
            data = struct.pack('<f', self.robot_cmd_data.latest['speed_vector']['vx'])
            send_data += data
            data = struct.pack('<f', self.robot_cmd_data.latest['speed_vector']['vy'])
            send_data += data
            data = struct.pack('<f', self.robot_cmd_data.latest['speed_vector']['wz'])
            send_data += data
            # print(len(send_data))
            
            # 添加chassis数据
            data = struct.pack('<f', self.robot_cmd_data.latest['chassis']['roll'])
            send_data += data
            data = struct.pack('<f', self.robot_cmd_data.latest['chassis']['pitch'])
            send_data += data
            data = struct.pack('<f', self.robot_cmd_data.latest['chassis']['yaw'])
            send_data += data
            data = struct.pack('<f', self.robot_cmd_data.latest['chassis']['leg_length'])
            send_data += data
            # print(len(send_data))
            
            # 添加gimbal数据
            data = struct.pack('<f', self.robot_cmd_data.latest['gimbal']['pitch'])
            send_data += data
            data = struct.pack('<f', self.robot_cmd_data.latest['gimbal']['yaw'])
            send_data += data
            # print(len(send_data))
            
            #添加shoot数据
            data = struct.pack('B', self.robot_cmd_data.latest['shoot']['fire'])
            send_data += data
            data = struct.pack('B', self.robot_cmd_data.latest['shoot']['fric_on'])
            send_data += data
            # print(len(send_data))
            
            #添加crc16校验
            send_data = crc.AppendCRC16(send_data)
            # print(len(send_data))
        elif send_id == SEND_ID_VIRTUAL_RC:
            send_data = b'\x5a\x1A\x03'
            send_data = crc.AppendCRC8(send_data)
            
            # 添加时间戳
            send_data += struct.pack('<I', self.timestamp)
            
            # 添加虚拟遥控器数据
            send_data += struct.pack('<h', data_dict['ch'][0])
            send_data += struct.pack('<h', data_dict['ch'][1])
            send_data += struct.pack('<h', data_dict['ch'][2])
            send_data += struct.pack('<h', data_dict['ch'][3])
            send_data += struct.pack('<h', data_dict['ch'][4])
            send_data += struct.pack('B', data_dict['s'][0])
            send_data += struct.pack('B', data_dict['s'][1])
            
            # 添加鼠标数据
            send_data += struct.pack('<h', 0)
            send_data += struct.pack('<h', 0)
            send_data += struct.pack('<h', 0)
            send_data += struct.pack('B', 0)
            send_data += struct.pack('B', 0)
            
            # 添加按键数据
            send_data += struct.pack('<H', 0)
            
            # 添加crc16校验
            send_data = crc.AppendCRC16(send_data)
            
        return send_data
    
    def clear(self):
        self.imu_data.clear()
        return